package actions;

import jason.asSemantics.DefaultInternalAction;
import jason.asSemantics.TransitionSystem;
import jason.asSemantics.Unifier;
import jason.asSyntax.Atom;
import jason.asSyntax.NumberTerm;
import jason.asSyntax.Term;
import jason.environment.grid.Location;

import java.awt.Point;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashSet;
import java.util.List;
import java.util.PriorityQueue;
import java.util.logging.Level;

import utils.Utils;
import ecomp.architecture.EcompArch;
import ecomp.models.LocalWorldModel;

public class astar extends DefaultInternalAction {

	/**
	 * 
	 */
	private static final long serialVersionUID = 4305843893258298443L;

	public LocalWorldModel model = null;

	Node[][] nodes = null;

	public astar() {
	}

	@Override
	public Object execute(TransitionSystem ts, Unifier un, Term[] args) throws Exception {
		try { 
			//long start = System.currentTimeMillis();
			int myX = (int)((NumberTerm)args[0]).solve();
			int myY = (int)((NumberTerm)args[1]).solve();
			int targetX = (int)((NumberTerm)args[2]).solve();
			int targetY = (int)((NumberTerm)args[3]).solve();
			int valorAbsoluto = (int)((NumberTerm)args[5]).solve();
			Location l = new Location(targetX,targetY);
			if(valorAbsoluto != 1){
				l = Utils.getClosestCell(myX, myY,targetX,targetY);
			}
			//Thread.sleep(100);
			if(myX == targetX && myY == targetY){
				return un.unifies(args[4], new Atom("skip"));
			}

			/*
                        //If dist is large, dont use AStar since it is costly
                        if(Math.abs(myX - targetX) + Math.abs(myY - targetY) > 30){                             
                                return un.unifies(args[4], new Atom("skip"));
                        }
			 */


			Solution sol = null;

			if(args.length == 6)
			{
				sol = AStarSearch.shortestPath(myX, myY, l.x, l.y, ts, false);
			}
			else
			{
				//sol = AStarSearch.search(myX, myY, l.x, l.y, ts, false);
				sol = AStarSearch.trueShortestPath(myX, myY, l.x, l.y, ts, false);
			}
			String action = "skip";

			if(sol.succeded()){
				action = sol.getFirstMove();
			}                       
			//delegate_target.t1 += end-start;
			return un.unifies(args[4], new Atom(action));

		} catch (Throwable e) {
			ts.getLogger().log(Level.SEVERE, "astar error: "+e, e);
		}
		return false;
	} // execute

	public static ArrayList<Node> getShortestPath(int startX, int startY, int endX, int endY,
			TransitionSystem ts, boolean fenceClear){
		//long start = System.currentTimeMillis();
		ArrayList<Node> temp = AStarSearch.shortestPath(startX, startY, endX, endY, ts, fenceClear).getPath();
		//long end = System.currentTimeMillis();
		//delegate_target.t1 += end-start;
		return temp;
	}

	//Returns the true shortest path without punishments
	public static Solution getShortestPath2(int startX, int startY, int endX, int endY,
			TransitionSystem ts, boolean fenceClear){
		//long start = System.currentTimeMillis();
		Solution temp = AStarSearch.trueShortestPath(startX, startY, endX, endY, ts, fenceClear);   
		//long end = System.currentTimeMillis();
		//delegate_target.t1 += end-start;
		return temp;
	}
}

class AStarSearch {

	//Returns shortest path with punishment for herders
	public static Solution search(int startX, int startY,
			int endX, int endY, TransitionSystem ts, boolean fenceClear){

		EcompArch arch = (EcompArch)ts.getUserAgArch();
		LocalWorldModel model = arch.getLocalWorldModel();

		Heuristic h = new Heuristic(model, endX, endY);

		HashSet<Node> closed = new HashSet<Node>();
		PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);


		Node initialNode = new Node(startX, startY, null, 0, h);
		fringe.add(initialNode);

		while(true){    
			if(fringe.isEmpty()){
				return new Solution();
			}
			Node n = fringe.poll();
			/*if(n.getX() == endX && n.getY() == endY)
				return new Solution(initialNode, n);*/
			
			if((n.getX() == endX && n.getY() == endY) || (Math.max(Math.abs(n.getX() - startX), Math.abs(n.getY()- startY)) > arch.lineOfSight))
				return new Solution(initialNode, n);

			if(!(closed.contains(n))){
				closed.add(n);
				//List<Node> children = n.expand(model, Node.PUNISHMENT, fenceClear);
				List<Node> children = n.expand(model, Node.PUNISHMENT_FELIPE, fenceClear);
				fringe.addAll(children);
			}                       
		}
	}

	//Returns the shortest path with special punishment for attackers
	public static Solution shortestPath(int startX, int startY,
			int endX, int endY, TransitionSystem ts, boolean fenceClear){

		EcompArch arch = (EcompArch)ts.getUserAgArch();
		LocalWorldModel model = arch.getLocalWorldModel();

		Heuristic h = new Heuristic(model, endX, endY);

		HashSet<Node> closed = new HashSet<Node>();
		PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);


		Node initialNode = new Node(startX, startY, null, 0, h);
		fringe.add(initialNode);

		while(true){    
			if(fringe.isEmpty()){
				return new Solution();
			}
			Node n = fringe.poll();
			/*if(n.getX() == endX && n.getY() == endY)
				return new Solution(initialNode, n);*/
			if((n.getX() == endX && n.getY() == endY) || (Math.max(Math.abs(n.getX() - startX), Math.abs(n.getY() - startY)) > arch.lineOfSight))
				return new Solution(initialNode, n);

			if(!(closed.contains(n))){
				closed.add(n);
				List<Node> children = n.expand(model, Node.PUNISHMENT_SPECIAL, fenceClear);
				fringe.addAll(children);
			}                       
		}               
	}

	//Returns the true shortest path
	public static Solution trueShortestPath(int startX, int startY,
			int endX, int endY, TransitionSystem ts, boolean fenceClear){

		EcompArch arch = (EcompArch)ts.getUserAgArch();
		LocalWorldModel model = arch.getLocalWorldModel();

		Heuristic h = new Heuristic(model, endX, endY);

		HashSet<Node> closed = new HashSet<Node>();
		PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);


		Node initialNode = new Node(startX, startY, null, 0, h);
		fringe.add(initialNode);

		while(true){    
			if(fringe.isEmpty()){
				return new Solution();
			}
			Node n = fringe.poll();
			/*if(n.getX() == endX && n.getY() == endY)
				return new Solution(initialNode, n);
*/
			if((n.getX() == endX && n.getY() == endY) || (Math.max(Math.abs(n.getX() - startX), Math.abs(n.getY() - startY)) > arch.lineOfSight))
				return new Solution(initialNode, n);
			if(!(closed.contains(n))){
				closed.add(n);
				List<Node> children = n.expand(model, Node.NO_PUNISHMENT, fenceClear);
				fringe.addAll(children);
			}                       
		}               
	}
} //AStarSearch

class Node implements Comparable<Node>{

	private int x,y;
	private Node parent;
	private Heuristic h;
	private double g;
	private double f;

	public Node(int x, int y, Node p, double g, Heuristic h){
		this.parent = p;
		this.g = g;
		this.h = h;
		this.x = x;
		this.y = y;
		f = g + h.heuristic(this);
	}
	
     public int compareTo(Node n) {
             if (this.f < n.f)
                     return -1;
             else if (this.f > n.f)
                     return 1;
             else
                     return 0;
     }

	public Node getParent(){
		return parent;
	}

	public static final int PUNISHMENT = 0;
	public static final int PUNISHMENT_SPECIAL = 1;
	public static final int NO_PUNISHMENT = 2;
	public static final int PUNISHMENT_FELIPE = 3;

	public List<Node> expand(LocalWorldModel model, int punishment, boolean fenceClear){
		List<Node> children = new ArrayList<Node>();            
		for(int i = -1; i <= 1; i++)
			for(int j = -1; j <= 1; j++)
				if(!(i == 0 && j == 0) &&
						i+x >= 0 && j+y >= 0 &&
						i+x < model.getWidth() && j+y < model.getHeight() &&
						isClear(x+i,y+j,model,fenceClear)){             
					Node child;
					if(punishment == PUNISHMENT_FELIPE){
						child = new Node(i+x,j+y,this,this.g + punishmentFelipe(model,x+i,y+j) + 1,h);
					}else if(punishment == PUNISHMENT){
						int newG = punishment(model,x+i,y+j);
						child = new Node(i+x,j+y,this,this.g + newG + 1,h);
					} else if(punishment == PUNISHMENT_SPECIAL){
						int newGSpecial = punishmentSpecial(model,x+i,y+j);
						child = new Node(i+x,j+y,this,this.g + newGSpecial + 1,h);
					}else{
						child = new Node(i+x,j+y,this,this.g + 1,h);
					}
					children.add(child);
				}
		return children;
	}

	private int punishment(LocalWorldModel model, int x, int y){
		int p = 0;

		Point cor = new Point(model.getCorralCenter().x, model.getCorralCenter().y);
		int xDir = x - cor.x;
		int yDir = y - cor.y;


		for(int i = -5; i <= 5; i++){
			for(int j = -5; j <= 5; j++){
				if(i+x >= 0 && i+x < model.getWidth() &&
						j+y >= 0 && j+y < model.getHeight()){
					if(model.hasObject(LocalWorldModel.COW, x+i, y+j)){
						//If cow is not between (x,y) and corral
						if(xDir*i > 0 || yDir*j > 0)
							p += 6 - Math.abs(i); //Punish harder if it gets closer to cow                                          
					}
				}
			}                       
		}

		if(model.hasObject(LocalWorldModel.AGENT, x, y))
			p += 5;

		return p;
	}

	private int punishmentSpecial(LocalWorldModel model, int x, int y){
		int p = 0;


		if(model.hasObject(LocalWorldModel.OURFENCE, x, y))
			p += 5;

		if(model.hasObject(LocalWorldModel.AGENT, x, y))
			p += 5;         
		if(model.hasObject(LocalWorldModel.CORRAL, x, y))
			p += 10;


		return p;
	}
	
	private int punishmentFelipe(LocalWorldModel model, int x, int y){
		int p = 0;

		p+= model.hasObjectPunish(LocalWorldModel.OURFENCE, x, y);
		p+= model.hasObjectPunish(LocalWorldModel.AGENT, x, y);
		p+= model.hasObjectPunish(LocalWorldModel.CORRAL, x, y);

		return p;
	}

	public String getMove(Node from, Node to) {
		if (to.x < from.x && to.y == from.y)
			return "west";
		else if (to.x > from.x && to.y == from.y)
			return "east";
		else if (to.x == from.x && to.y < from.y)
			return "north";
		else if (to.x > from.x && to.y < from.y)
			return "northeast";
		else if (to.x < from.x && to.y < from.y)
			return "northwest";
		else if (to.x == from.x && to.y > from.y)
			return "south";
		else if (to.x > from.x && to.y > from.y)
			return "southeast";
		else if (to.x < from.x && to.y > from.y)
			return "southwest";
		else
			return "skip";
	}

	private int getContent(int x, int y, LocalWorldModel model) {
		if (model.hasObject(LocalWorldModel.CLEAN, x, y))
			return LocalWorldModel.CLEAN;
		else if (model.hasObject(LocalWorldModel.AGENT, x, y))
			return LocalWorldModel.AGENT;
		else if (model.hasObject(LocalWorldModel.OBSTACLE, x, y))
			return LocalWorldModel.OBSTACLE;
		else if (model.hasObject(LocalWorldModel.COW, x, y))
			return LocalWorldModel.COW;
		else if (model.hasObject(LocalWorldModel.CORRAL, x, y))
			return LocalWorldModel.CORRAL;
		else if (model.hasObject(LocalWorldModel.ENEMY, x, y))
			return LocalWorldModel.ENEMY;
		else if (model.hasObject(LocalWorldModel.ENEMYCORRAL, x, y))
			return LocalWorldModel.ENEMYCORRAL;
		else if (model.hasObject(LocalWorldModel.FENCE, x, y))
			return LocalWorldModel.FENCE;
		else if (model.hasObject(LocalWorldModel.SWITCH, x, y))
			return LocalWorldModel.SWITCH;
		else if (model.hasObject(LocalWorldModel.OURFENCE, x, y))
			return LocalWorldModel.OURFENCE;

		return 0;
	}

	public boolean isClear(int x, int y, LocalWorldModel model, boolean fenceClear) {
		int content = getContent(x,y,model);
		return content != LocalWorldModel.OBSTACLE &&
				((content != LocalWorldModel.FENCE  && content != LocalWorldModel.OURFENCE)|| fenceClear) &&
				content != LocalWorldModel.SWITCH &&
				content != LocalWorldModel.ENEMY;
	}


	public double getG(){
		return g;
	}

	public int hashCode(){
		return (int)Math.pow(2, x) * (int)Math.pow(3, y);
	}

	public double getF(){
		return f;
	}

	public int getX(){
		return x;
	}

	public int getY(){
		return y;
	}

	public Heuristic getH(){
		return h;
	}


	public boolean equals(Object o){
		if(!(o instanceof Node))
			return false;
		Node n = (Node) o;
		return n.x == this.x && n.y == this.y;
	}

} //Node

class Solution {

	private boolean succes;
	private ArrayList<Node> path;

	public Solution(){
		path = new ArrayList<Node>();
		succes = false;
	}

	public Solution(Node initial, Node terminal){
		succes = true;

		path = new ArrayList<Node>();

		path.add(terminal);
		Node n = terminal;

		while(!n.equals(initial)){
			n = n.getParent();
			path.add(0, n);
		}       
	}

	public boolean succeded(){
		return succes;
	}

	public ArrayList<Node> getPath(){
		return path;
	}

	public int getPathLength(){
		return path.size()-1;
	}

	public String getFirstMove() {
		if(!succes)
			return "skip";
		int xFrom = path.get(0).getX();
		int yFrom = path.get(0).getY();
		int xTo = path.get(1).getX();
		int yTo = path.get(1).getY();

		if (xTo < xFrom && yTo == yFrom)
			return "west";
		else if (xTo > xFrom && yTo == yFrom)
			return "east";
		else if (xTo == xFrom && yTo < yFrom)
			return "north";
		else if (xTo > xFrom && yTo < yFrom)
			return "northeast";
		else if (xTo < xFrom && yTo < yFrom)
			return "northwest";
		else if (xTo == xFrom && yTo > yFrom)
			return "south";
		else if (xTo > xFrom && yTo > yFrom)
			return "southeast";
		else if (xTo < xFrom && yTo > yFrom)
			return "southwest";
		else
			return "skip";
	}
}

class Heuristic implements Comparator<Node>{

//	private LocalWorldModel model;
	private int endX, endY;

	public Heuristic(LocalWorldModel model, int endX, int endY){
//		this.model = model;
		this.endX = endX;
		this.endY = endY;
	}

	public double heuristic(Node n) {
		//return Math.max(Math.abs(n.getX() - endX), Math.abs(n.getY() - endY));
		return Math.sqrt(Math.pow(n.getX() - endX,2) + Math.pow(n.getY() - endY,2));
	}

	public int compare(Node n0, Node n1) {
		if(n0.getF() < n1.getF()) return -1;
		else if(n0.getF() > n1.getF()) return 1;
		return -1;
	}                               
};